#include <robot/robot.h>

#include <robot/kuka/carmclient.h>
#include <blackbox/connect.h>

namespace buola { namespace robot { namespace kuka {

CArmClient::CArmClient()
    :   BBox("arm client")
    ,   oJoints(this,"joints out",6)
    ,   iJoints(this,"joints in",6)
    ,   oPosition(this,"position out")
    ,   oOrientation(this,"orientation out")
    ,   oArmMode(this,"arm mode")
    ,   iStatus(this,"arm status")
{
}

CArmClient::~CArmClient()
{
}

void CArmClient::Init(const blackbox::CBoxProxy &pBox)
{
    mArm.reset(new blackbox::CBoxProxy(pBox));
    bb_connect(oJoints,pBox["joints in"]);
    bb_connect(oPosition,pBox["position in"]);
    bb_connect(oOrientation,pBox["orientation in"]);
    bb_connect(pBox["joints out"],iJoints);
    bb_connect(oArmMode,pBox["mode"]);
    bb_connect(pBox["status out"],iStatus);
}

void CArmClient::GetPose(std::vector<double> &pValues)
{
    PullAtomicInputs();
    pValues.clear();
    for(int i=0;i<6;i++)
        pValues.push_back(iJoints.Get(i));
}

void CArmClient::GetPose(CArmPose &pPose)
{
    std::vector<double> lJoints;
    GetPose(lJoints);
    pPose=CArmPose(lJoints);
}

void CArmClient::GetPose(mat::CVec3d &pPos,mat::C3DRotation &pRot)
{
    mat::CQuat_d lQuat;
    CArmPose lPose;
    GetPose(lPose);
    
    forward_kinematics(lPose,pPos,lQuat);
    pRot=mat::C3DRotation(lQuat);
}

mat::CVec3d CArmClient::GetPos()
{
    mat::CQuat_d lDummy;
    mat::CVec3d lPos;
    CArmPose lPose;
    GetPose(lPose);
    
    forward_kinematics(lPose,lPos,lDummy);
    return lPos;
}

mat::C3DRotation CArmClient::GetRot()
{
    mat::CVec3d lDummy;
    mat::CQuat_d lRot;
    CArmPose lPose;
    GetPose(lPose);
    
    forward_kinematics(lPose,lDummy,lRot);
    return mat::C3DRotation(lRot);
}

void CArmClient::SetGoal(const mat::CVec3d &pPos,bool pCartesian)
{
    SetGoal(pPos,GetRot(),pCartesian);
}

void CArmClient::SetGoal(const mat::C3DRotation &pRot,bool pCartesian)
{
    SetGoal(GetPos(),pRot,pCartesian);
}

void CArmClient::SetGoal(const mat::CVec3d &pPos,const mat::C3DRotation &pRot,bool pCartesian)
{
    if(pCartesian)
    {
        oPosition=pPos;
        oOrientation=mat::CQuat_d(pRot);
        oArmMode.Enable(MODE_GOAL_CARTESIAN);
        PushAtomicOutputs();
    }
    else
    {
        CArmPose lCurrent;
        GetPose(lCurrent);
        CArmPose lGoal;
        inverse_kinematics(pPos,mat::CQuat_d(pRot),lCurrent,lGoal);
        SetGoal(lGoal,false);
    }
}

void CArmClient::SetGoal(const CArmPose &pPose,bool pCartesian)
{
    if(pCartesian)
    {
        mat::CVec3d lPos;
        mat::CQuat_d lRot;
        forward_kinematics(pPose,lPos,lRot);
        SetGoal(lPos,mat::C3DRotation(lRot),true);
    }
    else
    {
        for(int i=0;i<6;i++)
            oJoints.Assign(i,pPose[i]);
        oArmMode.Enable(MODE_GOAL);
        PushAtomicOutputs();
    }
}

void CArmClient::SetGoal(const std::vector<double> &pJoints,bool pCartesian)
{
    SetGoal(CArmPose(pJoints),pCartesian);
}

void CArmClient::SetGoal(int pJoint,double pValue,bool pCartesian)
{
    CArmPose lPose;
    GetPose(lPose);
    lPose[pJoint]=pValue;
    SetGoal(lPose,pCartesian);
}

void CArmClient::DoCorrection(const mat::CVec3d &pPos,const mat::C3DRotation &pRot)
{
    oPosition=pPos;
    oOrientation=mat::CQuat_d(pRot);
    oArmMode.Enable(MODE_CORRECTION_CARTESIAN);
    PushAtomicOutputs();
}

void CArmClient::Stop()
{
    oArmMode.Disable();
    PushAtomicOutputs();
}

void CArmClient::StartSinTest(int pJoint,double pAmplitude)
{
    for(int i=0;i<6;i++)
        oJoints.Assign(i,(i==pJoint)?pAmplitude:0);
    oArmMode.Enable(MODE_SIN_TEST);
    PushAtomicOutputs();
}

void CArmClient::SetMaxSpeed(double pLinear,double pRot)
{
    mArm->Call<void(double,double)>("set max speed",pLinear,pRot);
}

bool CArmClient::IsMoving()
{
    PullAtomicInputs();
    return iStatus.Get();
}

void CArmClient::WaitForStop()
{
    sleep(chrono::milliseconds(120));
    while(IsMoving())
        sleep(chrono::milliseconds(10));
}

/*namespace kuka*/ } /*namespace robot*/ } /*namespace buola*/ }
